
// import oimo.common.Transform;
// import haxe.ds.Array;
// import oimo.collision.broadphase.*;
// import oimo.collision.geometry.Aabb;
// import oimo.collision.geometry.ConvexGeometry;
// import oimo.common.Setting;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { BroadPhase } from "../BroadPhase";
import { BvhTree } from "./BvhTree";
import { BvhProxy } from "./BvhProxy";
import { BroadPhaseType } from "../BroadPhaseType";
import { M } from "../../../common/M";
import { Aabb } from "../../geometry/Aabb";
import { Vec3 } from "../../../common/Vec3";
import { BvhNode } from "./BvhNode";
import { BroadPhaseProxyCallback } from "../BroadPhaseProxyCallback";
import { ConvexGeometry } from "../../geometry/ConvexGeometry";
import { Transform } from "../../../common/Transform";
import { Setting } from "../../../common/Setting";
import { Proxy } from "../Proxy";

/**
 * The broad-phase collision detection algorithm based on bounding volume hierarchy (BVH).
 * Average time complexity is O(NlogN) or lower.
 */
export class BvhBroadPhase extends BroadPhase {
	_tree: BvhTree;

	movedProxies: Array<BvhProxy>;
	numMovedProxies: number;


	constructor() {
		super(BroadPhaseType.BVH);
		this._incremental = true;
		this._tree = new BvhTree();
		this.movedProxies = new Array<BvhProxy>(1024);
		this.numMovedProxies = 0;
	}

	// --- ---


	private addToMovedProxy(bvhProxy: BvhProxy): void {
		// add to the buffer
		if (bvhProxy._moved) return;
		bvhProxy._moved = true;

		this.movedProxies[this.numMovedProxies++] = bvhProxy;
	}


	private updateProxy(p: BvhProxy, aabb: Aabb, displacement: Vec3): void {
		// set tight AABB
		p._setAabb(aabb);

		// fatten the AABB
		var padding: number = Setting.bvhProxyPadding;
		var paddingVec: Vec3 = new Vec3();
		M.vec3_set(paddingVec, padding, padding, padding);
		M.vec3_sub(p._aabbMin, p._aabbMin, paddingVec);
		M.vec3_add(p._aabbMax, p._aabbMax, paddingVec);

		if (displacement != null) {
			// predict movement
			var d: Vec3 = new Vec3();
			var zero: Vec3 = new Vec3();
			var addToMin: Vec3 = new Vec3();
			var addToMax: Vec3 = new Vec3();
			M.vec3_zero(zero);
			M.vec3_fromVec3(d, displacement);
			M.vec3_min(addToMin, zero, d);
			M.vec3_max(addToMax, zero, d);
			M.vec3_add(p._aabbMin, p._aabbMin, addToMin);
			M.vec3_add(p._aabbMax, p._aabbMax, addToMax);
		}
	}

	public collide(n1: BvhNode, n2: BvhNode): void {
		this._testCount++;
		var l1: boolean = n1._height == 0;
		var l2: boolean = n2._height == 0;
		if (n1 == n2) {
			if (l1) return;
			this.collide(n1._children[0], n2);
			this.collide(n1._children[1], n2);
			return;
		}
		if (!M.aabb_overlap(n1._aabbMin, n1._aabbMax, n2._aabbMin, n2._aabbMax)) {
			return;
		}
		if (l1 && l2) {
			this.pickAndPushProxyPair(n1._proxy, n2._proxy);
			return;
		}
		if (l2 || n1._height > n2._height) {
			// descend node 1
			this.collide(n1._children[0], n2);
			this.collide(n1._children[1], n2);
		} else {
			// descend node 2
			this.collide(n2._children[0], n1);
			this.collide(n2._children[1], n1);
		}
	}

	public rayCastRecursive(node: BvhNode, _p1: Vec3, _p2: Vec3, callback: BroadPhaseProxyCallback): void {
		// TODO: use stack?
		var p1: Vec3 = new Vec3();
		var p2: Vec3 = new Vec3();
		M.vec3_assign(p1, _p1);
		M.vec3_assign(p2, _p2);

		if (!(this.aabbSegmentTest(node._aabbMin, node._aabbMax, p1, p2))) {
			return;
		}

		if (node._height == 0) { // leaf
			callback.process(node._proxy);
			return;
		}

		(this.rayCastRecursive(node._children[0], p1, p2, callback));
		(this.rayCastRecursive(node._children[1], p1, p2, callback));
	}

	public convexCastRecursive(node: BvhNode, convex: ConvexGeometry, begin: Transform, translation: Vec3, callback: BroadPhaseProxyCallback): void {
		// TODO: use stack?
		if (!(this.aabbConvexSweepTest(node._aabbMin, node._aabbMax, convex, begin, translation))) {
			return;
		}

		if (node._height == 0) { // leaf
			callback.process(node._proxy);
			return;
		}

		(this.convexCastRecursive(node._children[0], convex, begin, translation, callback));
		(this.convexCastRecursive(node._children[1], convex, begin, translation, callback));
	}

	public aabbTestRecursive(node: BvhNode, aabb: Aabb, callback: BroadPhaseProxyCallback): void {
		if (!M.aabb_overlap(node._aabbMin, node._aabbMax, aabb._min, aabb._max)) {
			return;
		}

		if (node._height == 0) { // leaf
			callback.process(node._proxy);
			return;
		}

		this.aabbTestRecursive(node._children[0], aabb, callback);
		this.aabbTestRecursive(node._children[1], aabb, callback);
	}

	// --- public ---

	public createProxy(userData: any, aabb: Aabb): Proxy {
		var p: BvhProxy = new BvhProxy(userData, this._idCount++);
		this.addProxy(p);

		this.updateProxy(p, aabb, null);
		this._tree._insertProxy(p);
		this.addToMovedProxy(p);

		return p;
	}

	public destroyProxy(proxy: Proxy): void {
		this.removeProxy(proxy);

		var bvhProxy: BvhProxy = proxy as BvhProxy;
		this._tree._deleteProxy(bvhProxy);
		bvhProxy.userData = null;
		bvhProxy._next = null;
		bvhProxy._prev = null;

		if (bvhProxy._moved) {
			bvhProxy._moved = false;
		}
	}

	public moveProxy(proxy: Proxy, aabb: Aabb, displacement: Vec3): void {
		var p: BvhProxy = proxy as BvhProxy;
		if (M.aabb_contains(p._aabbMin, p._aabbMax, aabb._min, aabb._max)) {
			// need not move proxy
			return;
		}

		this.updateProxy(p, aabb, displacement);
		this.addToMovedProxy(p);
	}

	public collectPairs(): void {
		this.poolProxyPairs();
		this._testCount = 0;
		if (this._numProxies < 2) return;

		var topDown: boolean = false;

		if (topDown) {
			while (this.numMovedProxies > 0) {
				this.movedProxies[--this.numMovedProxies] = null;
			}
			this._tree._buildTopDown();
			this.collide(this._tree._root, this._tree._root);
			return;
		}

		var incrementalCollision: boolean = this.numMovedProxies / this._numProxies < Setting.bvhIncrementalCollisionThreshold;

		// incremental modification
		for (let i = 0; i < this.numMovedProxies; ++i) {
			var p: BvhProxy = this.movedProxies[i];
			if (p._moved) {
				this._tree._deleteProxy(p);
				this._tree._insertProxy(p);
				if (incrementalCollision) {
					this.collide(this._tree._root, p._leaf);
				}
				p._moved = false;
			}
			this.movedProxies[i] = null;
		}
		if (!incrementalCollision) {
			this.collide(this._tree._root, this._tree._root);
		}

		this.numMovedProxies = 0;
	}

	public rayCast(begin: Vec3, end: Vec3, callback: BroadPhaseProxyCallback): void {
		if (this._tree._root == null) return; // no AABBs in the broadphase

		var p1: Vec3 = new Vec3();
		var p2: Vec3 = new Vec3();
		M.vec3_fromVec3(p1, begin);
		M.vec3_fromVec3(p2, end);

		(this.rayCastRecursive(this._tree._root, p1, p2, callback));
	}

	public convexCast(convex: ConvexGeometry, begin: Transform, translation: Vec3, callback: BroadPhaseProxyCallback): void {
		if (this._tree._root == null) return; // no AABBs in the broadphase

		(this.convexCastRecursive(this._tree._root, convex, begin, translation, callback));
	}

	public aabbTest(aabb: Aabb, callback: BroadPhaseProxyCallback): void {
		if (this._tree._root == null) return; // no AABBs in the broadphase

		this.aabbTestRecursive(this._tree._root, aabb, callback);
	}

	/**
	 * Returns the balance of the bounding volume tree.
	 */
	public getTreeBalance(): number {
		return this._tree._getBalance();
	}
}
